#include "abstractsystem.h"
#include <QDebug>

AbstractSystem::AbstractSystem()
{
    pos = NULL;
}

AbstractSystem::~AbstractSystem()
{


    for(int i=0; i<motors.length(); i++){
        if(motors[i]->isRunning())
            motors[i]->quit();
        delete motors[i];
    }
    for(int i=0; i<sensors.length(); i++)
        delete sensors[i];
    delete pos;
}

QList<double> AbstractSystem::state()
{
    QList<double> result;
    for(int i=0; i<sensors.size(); i++)
        result << sensors[i]->getValue();
    //result << pos->getValues();
    return(result);
}

void AbstractSystem::setConsign(QList<double> cons)
{
    for(int i=0; i<motors.size(); i++)
        motors[i]->setConsign(cons[i]);
}

double AbstractSystem::getMotorState(int id)
{
    return(motors[id]->getState());
}

void AbstractSystem::setMotorState(int id, double s)
{
    motors[id]->setState(s);
    checkValue(id);
}

void AbstractSystem::setPosition(double x, double y, double theta)
{
    (void) x;
    (void) y;
    (void) theta;
}

void AbstractSystem::addPositionSensor(int freq, double delay, double imprecision)
{
    pos = new PositionSensor(this, freq, delay, imprecision);
}
